#include "LidarDriverX.h"
#include "PointCloudHandler.h"

#pragma region Init
LidarDriverX::LidarDriverX(LidarConfigurationOptions *options, int id)
    : _options(options), _lidar_id(id)
{
    updateDriverParam(options);
}

LidarDriverX::~LidarDriverX()
{
    stop();
    _pointCloudDataCallback.clear();
}
 
void LidarDriverX::start()
{
    init();
    _to_exit_process = false;
    _threadPointCloudProcess.setName("PointCloudProcess");
    _threadPointCloudProcess.start(&LidarDriverX::threadPointCloudProcess, this);
}

void LidarDriverX::stop()
{
    _to_exit_process = true;
    dispose();
    _threadPointCloudProcess.tryJoin(5000);
}
#pragma endregion

#pragma region Thread
bool LidarDriverX::addPointCloudMsgCallback(PointCloudDataCallback callback)
{
    _pointCloudDataCallback.push_back(callback);
    return true;
}

void LidarDriverX::threadPointCloudProcess(void *pContext)
{
    LidarDriverX *pHandler = static_cast<LidarDriverX *>(pContext);
    pHandler->processPointCloud();
}

void LidarDriverX::updateDriverParam(LidarConfigurationOptions *options)
{
    _options = options;
    _decodeType = std::get<LidarDecodeType>(options->GetValue(LidarConfigurationOptions::Data_Callback_DecType));
    _transformParam = std::get<TransformParam>(options->GetValue(LidarConfigurationOptions::Data_Decode_Transform));

    filt_meank = std::get<float>(options->GetValue(LidarConfigurationOptions::Data_Decode_FiltMeanK));
    filt_threshold = std::get<float>(options->GetValue(LidarConfigurationOptions::Data_Decode_FiltThreshold));
    sample_voxel_size = std::get<float>(options->GetValue(LidarConfigurationOptions::Data_Decode_SampleVoxel));

    is_noDelay = std::get<bool>(options->GetValue(LidarConfigurationOptions::Data_Callback_NoDelay));
    if (is_noDelay)
        max_buffer_num = 1;
}
#pragma endregion

#pragma region Method
void LidarDriverX::decodePointCloud(PointCloudData<pcl::PointXYZI> &data, pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud)
{
    pcl_pointcloud = PointCloudHandler::GetTransformCloud(pcl_pointcloud, _transformParam);
    switch (_decodeType)
    {
    case LidarDecodeType::FILTERED:
        data.cloud_ptr = PointCloudHandler::GetFilteredCloud(pcl_pointcloud, filt_meank, filt_threshold);
        break;
    case LidarDecodeType::DOWNSAMPLED:
        data.cloud_ptr = PointCloudHandler::GetDownSampledCloud(pcl_pointcloud, sample_voxel_size, sample_voxel_size, sample_voxel_size);
        break;
    default:
        data.cloud_ptr = pcl_pointcloud;
        break;
    }

    auto now = std::chrono::system_clock::now();
    auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
    data.timestamp = (double)timestamp / 1000.0;
}
#pragma endregion